#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <MotorShield.h>
#include <HC_SR04.h>
#include <TimerThree.h>
#include <TimerOne.h>
#include <FlameSensor.h>
#include <Goertzel.h>

// Creates the motor shield object with the default I2C address
MotorShield ms1 = MotorShield(3,4,1,2);

//ultrasonic objects
HC_SR04 us1(30,31);
HC_SR04 us2(32,33);
HC_SR04 us3(34,35);
HC_SR04 us4(36,37);
HC_SR04 us5(38,39);
HC_SR04 us6(40,41);

// Creates flame sensor object
FlameSensor fs1(A0);
int newReading=0;
int oldReading=0;

// Creates push button variables
int pbIn = 0; // Interrupt 0 is on DIGITAL PIN 2!

// extinguisher pin
int fanPin=45;

// Creates microphone variables
int sensorPin = A1;
const float TARGET_FREQUENCY1 = 3800; 
const float TARGET_FREQUENCY2 = 2500; 
const int N = 100;
const float THRESHOLD1 = 4000;
const float THRESHOLD2 = 4000;
const float SAMPLING_FREQUENCY = 8900;
Goertzel goertzel1 = Goertzel(TARGET_FREQUENCY1, N, SAMPLING_FREQUENCY);
Goertzel goertzel2 = Goertzel(TARGET_FREQUENCY2, N, SAMPLING_FREQUENCY);

//debug variable
//when true robot should face "forward" in the starting position of the maze
bool frontFacing = true;

bool usesCm = false;

int NINETY_DEGREE_DELAY = 1020; // NEW BATTERIES: ~940 FOR 360DEG TURN
int NINETY_DEGREE_SPEED = 128;
int FORWARD_3INCHES_DELAY = 125;
int FORWARD_1INCHES_DELAY = 25;
int FORWARD_1INCHES_SPEED=20;
int FORWARD_3INCHES_SPEED = 90;

int speed_fast=64, speed_slow=32, turn_speed=128;
//the delay need to move either one cm or one inch
//defined in setup
int UNIT_DISTANCE_DELAY = -1;
int CM_DISTANCE_DELAY = 50; //TODO: Measure this 
int INCHES_DISTANCE_DELAY = 60; //TODO: Measure this
int maxWallDistance = -1;
int angle_slow_offset = -64;

long d1, d2, d3, d4, d5, d6, diff23;

long cornerWallDistance;
long WALL_REACHED_DISTANCE;
long ROBOT_LENGTH = 20/2.54; 
boolean cornerWallDetected = false, startup=true, activated=false, flameFound=false, flameRight=true, flameLeft=false, facingCandle=false;
boolean corrected=false, right=false, left=false, priorRight=false, priorLeft=false;
boolean isFast=false, isSlow=false, isMoving=false, isVeerRight=false, isVeerLeft=false;
int led=13, count=0;
int rightTurns=0;

int readDiff=0, readDelay=30;

//------------------------------------------------------------------------------------------
void setup(){
  Serial.begin(9600);
  Timer3.initialize();
  ms1.begin();
  ms1.stop();
  //Wire.begin();
  //fs1.setDebugOn();
  fs1.setDebugOff();
  pinMode(led, OUTPUT);
  pinMode(fanPin, OUTPUT);
  // button interrupt
  attachInterrupt(pbIn, buttonInput, RISING);
  // microphone interrupt
  Timer3.attachInterrupt(goertzel, 125000);
  
  if(usesCm){
    UNIT_DISTANCE_DELAY = CM_DISTANCE_DELAY;
    maxWallDistance = 30;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 5;
    ROBOT_LENGTH = 20; //TODO: Measure this
  }
  else{
    UNIT_DISTANCE_DELAY = INCHES_DISTANCE_DELAY;
    maxWallDistance = 30/2.54;//TODO:  NEED TO MEASURE
    WALL_REACHED_DISTANCE = 7; //4
    ROBOT_LENGTH = 20/2.54; //TODO: Measure this
  }
  /*Serial.println("3");
  delay(1000);
  Serial.println("2");
  delay(1000);
  Serial.println("1");
  delay(1000);
  Serial.println("LIFTOFF");*/
}
//------------------------------------------------------------------------------------------
void loop(){
  if(activated){    
    if(startup){
      ms1.stop();
      Serial.println("startup right turn");

      ms1.turnRight(turn_speed);
      delay(NINETY_DEGREE_DELAY);
      ms1.stop();
      
      startup=false;
    }
    
    // this will check to see if the robot sees the candle
    if(fs1.isFlame()){
      flameFound = true;
      /*newReading = fs1.getSensorValue();
      oldReading = newReading;
      ms1.turnRight(speed_fast);
      flameRight = true;*/
    }
    if(flameFound){
      /*newReading = fs1.getSensorValue();
      flame_found();*/
      flame_found2();
    }
    else{
        // do everything else
      
      //////////////////////////
      //check if there is a wall immediately in front of the robot
      //////////////////////////
      d1 = us1.getDistance(usesCm);
      delayMicroseconds(50);
      
      // this checks if enough right turns have been made to check for room 4
      if(rightTurns >= 10 && d1 > (2*maxWallDistance)){
        // check for a corner to the left
        d6 = us6.getDistance(usesCm);
        delayMicroseconds(50);
        d5 = us5.getDistance(usesCm);
        
        while(d6 > maxWallDistance && d6 > (d5 + maxWallDistance)){
          //if you got to here in the code,
          //you are at a corner: meaning us2 can see into a room but us3 has a wall in front of it
          cornerWallDetected = true;
        
          //robot inches forward until past the wall
          cornerWallDistance = d5;
        		
          //robot inches forward
          if(!isSlow){
            ms1.stop();
            Serial.println("moving slowly");
            ms1.moveForward(speed_slow);
            isSlow = true;
            isFast = false;
          }
          
          //get right side sensors
          d6 = us6.getDistance(usesCm);
          delayMicroseconds(50);
          d5 = us5.getDistance(usesCm);
        }
        
        //if robot just got done inching forward to get past a wall, inch forward a little more one more time
        if(cornerWallDetected){
          //robot inches forward
          ms1.stop();
          Serial.println("inching forward once");
          ms1.moveForward(speed_fast);
          delay(FORWARD_1INCHES_DELAY + 80);
          ms1.stop();
          isFast = false;
          isSlow = false;
          cornerWallDetected = false;
        }
        // now both d2 and d3 are past the corner
        if(d6 > maxWallDistance && d5 > maxWallDistance){
          //if you got to here in the code,
          //there is empty space to the right of the robot
          ms1.stop();
          Serial.println("turning left");
          ms1.turnLeft(turn_speed);
          delay(NINETY_DEGREE_DELAY);
          ms1.stop();
          
          ms1.moveForward(128); // NOTE: 128 SHOULD BE speed_fast
          //delay((cornerWallDistance * UNIT_DISTANCE_DELAY) + (ROBOT_LENGTH * UNIT_DISTANCE_DELAY));
          delay((cornerWallDistance * UNIT_DISTANCE_DELAY) + (ROBOT_LENGTH * UNIT_DISTANCE_DELAY) + 80);
          ms1.stop();
          isFast = false;
          isSlow = false;
          
          // this left turn enters room 4
          // reset right turn counter
          rightTurns = -1;
        }
      }
            
      if(d1 < WALL_REACHED_DISTANCE){
        ms1.stop();
        Serial.println("turning left");
        ms1.turnLeft(turn_speed);
        delay(NINETY_DEGREE_DELAY);
        ms1.stop();
        
        isFast=false;
        isSlow=false;
        return;
      }
      else{
        //move forward, but not too much
        // commenting out this forward to use if statements
        //compare sensor readings us2 and us3; diff23 is absolute value of difference
        // if the distance is greater than #in and less than 3.5in, angle correct before moving forward
        angle_correction_moving();
      }
      
      //////////////////////////
      //check if there is space to the right of the robot
      //////////////////////////
      //get right side sensors
      d2 = us2.getDistance(usesCm);
      delayMicroseconds(50);
      d3 = us3.getDistance(usesCm);
      
      //Measure distance from US2/3 to wall when US5/6 are up against wall
      while(d2 > maxWallDistance && d2 > (d3 + maxWallDistance)){
        //if you got to here in the code,
        //you are at a corner: meaning us2 can see into a room but us3 has a wall in front of it
        cornerWallDetected = true;
      
        //robot inches forward until past the wall
        cornerWallDistance = d3;
      		
        //robot inches forward
        if(!isSlow){
          ms1.stop();
          Serial.println("moving slowly");
          ms1.moveForward(speed_slow);
          isSlow = true;
          isFast = false;
        }
        
        //get right side sensors
        d2 = us2.getDistance(usesCm);
        delayMicroseconds(50);
        d3 = us3.getDistance(usesCm);
      }
      
      //if robot just got done inching forward to get past a wall, inch forward a little more one more time
      if(cornerWallDetected){
        //robot inches forward
        ms1.stop();
        Serial.println("inching forward once");
        ms1.moveForward(speed_fast);
        delay(FORWARD_1INCHES_DELAY + 80);
        ms1.stop();
        isFast = false;
        isSlow = false;
        cornerWallDetected = false;
      }
      
      // now both d2 and d3 are past the corner
      if(d2 > maxWallDistance && d3 > maxWallDistance){
        //if you got to here in the code,
        //there is empty space to the right of the robot
        ms1.stop();
        Serial.println("turning right");
        ms1.turnRight(turn_speed);
        delay(NINETY_DEGREE_DELAY);
        ms1.stop();
        rightTurns += 1;
        Serial.print("# of right turns: ");
        Serial.println(rightTurns);
        
        ms1.moveForward(128); // NOTE: 128 SHOULD BE speed_fast
        //delay((cornerWallDistance * UNIT_DISTANCE_DELAY) + (ROBOT_LENGTH * UNIT_DISTANCE_DELAY));
        delay((cornerWallDistance * UNIT_DISTANCE_DELAY) + (ROBOT_LENGTH * UNIT_DISTANCE_DELAY) + 80);
        ms1.stop();
        isFast = false;
        isSlow = false;
      	
        //NOTE:
        /*
        this portion of the code can be reached during a double corner, in which case, both d2 and d3 COULD be very large.
        If that is the case, it will turn right and go forward with the same distance that was used on the last corner
        */
      }
    }
  }
}
//------------------------------------------------------------------------------------------
int time1=1750, speed1=90;
void flame_found2(){
  ms1.stop();
  
  digitalWrite(fanPin, HIGH);
  ms1.turnLeft_spin(speed1);
  delay(time1);
  ms1.stop();
  ms1.turnRight(speed1);
  delay(2*time1);
  ms1.stop();
  ms1.turnLeft_spin(speed1);
  delay(time1);
  ms1.stop();
  
  digitalWrite(fanPin, LOW);
  newReading = fs1.getSensorValue();
  if(newReading == 0){
    activated = false;
    flameFound = false;
    return;
  }
  else{
    ms1.moveForward(speed1);
    delay(500);
    ms1.stop();
  }
}
//------------------------------------------------------------------------------------------
//before you do this the first time, get an old reading and start turning in some direction
//and turn on the fan
void flame_found(){
  if(newReading < 3){
    // flame is extinguished
    ms1.stop();
    //digitalWrite(fanPin, LOW);
  }
  else if(newReading < oldReading){
   //change directions
   if(flameRight){
     ms1.stop();
     ms1.turnLeft(speed_fast);
     //delay(readDelay);
     flameRight=false;
     flameLeft=true;
     
     readDiff = newReading - oldReading;
     readDiff = abs(readDiff);
     if(readDiff > 5){
       oldReading = newReading;
     }
   }
   else{
     ms1.stop();
     ms1.turnRight(speed_fast);
     //delay(readDelay);
     flameLeft=false;
     flameRight=true;
     
     readDiff = newReading - oldReading;
     readDiff = abs(readDiff);
     if(readDiff > 5){
       oldReading = newReading;
     }
   }
  }
  
}
//------------------------------------------------------------------------------------------
void angle_correction_moving(){
  d2 = us2.getDistance(usesCm);
  delayMicroseconds(50);
  d3 = us3.getDistance(usesCm);
  diff23 = d2 - d3;
  diff23 = abs(diff23);
  //if(diff23 > 0.75 && diff23 < 4){
    // need to offset right
    if((d2>d3 && diff23>0.5 && d2>3 && d3>3) || (d2>5 && d3>5)){
      ms1.veerRight(128);
    }
    // else need to offset left
    else if((d3>d2 && diff23>0.5) || (d2<3 && d3<3)){
      ms1.veerLeft(128);
    }
    else{
      ms1.moveForward(128);
    }
}
//------------------------------------------------------------------------------------------
// this is the button interrupt function
void buttonInput(){
  Serial.println("button pressed");
  activated=true;
  // remove button interrupt and mic interrupt so debounce won't crash program
  detachInterrupt(pbIn);
  Timer3.detachInterrupt();
}
//------------------------------------------------------------------------------------------
// these are the microphone interrupt functions
void goertzel(){
  twoFiveFreq();
  //threeEightFreq();
}
void twoFiveFreq(){
  goertzel2.sample(sensorPin); //Will take n samples
  float magnitude2 = goertzel2.detect();  //check them for target_freq
  if(magnitude2>THRESHOLD2){ //if you're getting false hits or no hits adjust this
    activated=true;
    Serial.println("2.5KHz FOUND");
    detachInterrupt(pbIn);
    Timer3.detachInterrupt();
  }
}
void threeEightFreq(){
  goertzel1.sample(sensorPin); //Will take n samples
  float magnitude1 = goertzel1.detect();  //check them for target_freq
  if(magnitude1>THRESHOLD1){ //if you're getting false hits or no hits adjust this
    activated=true;
    Serial.println("3.8KHz FOUND");
    detachInterrupt(0);
    Timer3.detachInterrupt();
  }
}
//------------------------------------------------------------------------------------------
/**
set the "front" face of the robot
*/
void calibratePos(){
  if(frontFacing){
    //assign default ultrasonic sensors
    //us1 ---> (30,31) etc.
  }
  else{
    //scan sensors
		
    //assign us1-us6 based on info
		
    //turn robot so that it is lengthwise in the hallway
  }
  
}
//------------------------------------------------------------------------------------------
